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Abstract —We address the optimal dynamic formation prob¬ 
lem in mobile leader-follower networks where an optimal 
formation is generated to maximize a given objective function 
while continuously preserving connectivity. We show that in 
a convex mission space, the connectivity constraints can be 
satisfied by any feasible solution to a mixed integer nonlinear 
optimization problem. When the optimal formation objective 
is to maximize coverage in a mission space cluttered with 
obstacles, we separate the process into intervals with no 
obstacles detected and intervals where one or more obstacles 
are detected. In the latter case, we propose a minimum-effort 
reconfiguration approach for the formation which still optimizes 
the objective function while avoiding the obstacles and ensuring 
connectivity. We include simulation results illustrating this 
dynamic formation process. 

1. Introduction 

The multi-agent system framework consists of a team 
of autonomous agents cooperating to carry out complex 
tasks within a given environment that is potentially highly 
dynamic, hazardous, and even adversarial. The overall ob¬ 
jective of the system may be time-varying and combines 
exploration, data collection, and tracking to define a “mis¬ 
sion”. Related problems are often referred to as multi¬ 
agent coordination [l]-[3] or cooperative control [4]-[6]. 
In many cases, mobile agents are required to establish and 
maintain a certain spatial configuration, leading to a variety 
of formation control problems. These problems are generally 
approached in two ways: in the leader-follower setting, an 
agent is designated as a team leader moving on some given 
trajectory with the remaining agents tracking this trajectory 
while maintaining the formation; in the leaderless setting 
the formation must be maintained without any such benefit. 
Examples of formation control problems may be found in 
[7], [8], [9]-[12] and references therein. In robotics, this 
is a well-studied problem; for instance in [10], a desired 
shape for a networked strongly connected group of robots 
is achieved by designing a quadratic spread potential field 
on a relative distance space. In [9], a leader and several 
followers move in an area with obstacles which necessitate 
the transition from an initial formation shape to a desired 
new shape; however, the actual choice of formations for a 
particular mission is not addressed in [9], an issue which 
is central to our approach in this paper. In [12] the authors 
consider the problem of preserving connectivity when the 
nodes have limited sensing and communication ranges; this 
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is accomplished through a control law based on the gradient 
of an edge-tension function. More recently, in [11], the goal 
is to integrate formation control with trajectory tracking and 
obstacle avoidance using an optimal control framework. 

In this paper, we take a different viewpoint of forma¬ 
tions. Since agent teams are typically assigned a mission, 
there is an objective (or cost) function associated with the 
team’s operation which depends on the spatial configuration 
(formation) of the team. Therefore, we view a formation 
as the result of an optimization problem which the agent 
team solves in either centralized or distributed manner. We 
adopt a leader-follower approach, whereby the leader moves 
according to a trajectory that only he/she controls. During 
the mission, the formation is preserved or must adapt if the 
mission (hence the objective function) changes or if the com¬ 
position of the team is altered (by additions or subtractions 
of agents) or if the team encounters obstacles which must be 
avoided. In the latter case in particular, we expect that the 
team adapts to a new formation which still seeks to optimize 
an objective function so as to continue the team’s mission 
by attaining the best possible performance. The problem is 
complicated by the fact that such adaptation must take place 
in real time. Thus, if the optimization problem determining 
the optimal formation is computationally demanding, we 
must seek a fast and efficient control approach which yields 
possibly suboptimal formations, but guarantees that the initial 
connectivity attained is preserved. Obviously, once obstacles 
are cleared, the team is expected to return to its nominal 
optimal formation. 

Although the optimal dynamic formation control frame¬ 
work proposed here is not limited by the choice of tasks 
assigned to the team, we will focus on the coverage control 
problem because it is well studied and amenable to efficient 
distributed optimization methods [6], [13]-[19], while also 
presenting the challenge of being generally non-convex and 
sensitive to the agent locations during the execution of a 
mission. The local optimality issue, which depends on the 
choice of objective function, is addressed in [20]-[22], while 
the problem of connectivity preservation in view of limited 
communication ranges is considered in [12], [18]. 

The contribution of this paper is to formulate an op¬ 
timization problem which jointly seeks to position agents 
in a two-dimensional mission space so as to optimize a 
given objective function while at the same time ensuring 
that the leader and remaining agents maintain a connected 
graph dictated by minimum distances between agents, thus 
resulting in an optimal formation. The minimum distances 
may capture limited communication ranges as well as any 



other constraint imposed on the team. We show that the 
solution to this problem guarantees this connectivity. The 
formation becomes dynamic as soon as the leader starts 
moving along a given trajectory which may either be known 
to all agents in advance or determined only by the leader. 
Thus, it is the team’s responsibility to maintain an optimal 
formation. We show that this is relatively simple as long as 
no obstacles are encountered. When one or more obstacles 
are encountered (i.e., they come within the sensing range of 
one or more agents), then we propose a scheme for adapting 
with minimal effort to a new formation which maintains 
connectivity while still seeking to optimize the original team 
objective. 

The paper is organized as follows. In Sec. II, we formulate 
a general optimal formation control problem. In Sec. Ill, 
we focus on a convex feasible space and derive a mixed 
integer nonlinear problem whose solution is shown to ensure 
connectivity while maintaining an optimal formation. In Sec. 
IV, we propose a scheme to solve the optimal formation 
problem in a mission space with obstacles. We propose an 
algorithm to first obtain a connected formation and then 
optimize it while maintaining connectivity. Simulation results 
are included in Sec. V. 

II. Optimal Formation Problem Formulation 

Consider a set of V +1 agents with a leader labeled 0 and 
N followers labeled 1 through V in a mission space G 
Agent i is located at Si{t) G M? and let s{t) = (5'o(0: ••• Aa(0) 
be the full agent location vector at t. The leader follows a 
predefined trajectory so{t) over t e [ 0 , T] which is generally 
not known in advance by the remaining agents. We model the 
agent team as a undirected graph ^(s) = where 

^ = {0,1, ...,V} is the set of agent indices and let = 
C ^ be the set of follower indices. In this model, 
the set of edges S’ = {{ij) : ij G contains all possible 
agent pairs for which constraints may be imposed. 

In performing a mission, let be an objective func¬ 

tion dependent on the agent locations s{t). If the locations 
are unconstrained, the problem is posed as maXs(^)^^Ff(s(r)) 
subject to dynamics that may characterize the motion of 
each agent. If t is fixed, then this is a nonlinear parametric 
optimization problem over the mission space Cl [18]. If, 
on the other hand, agents are required to also satisfy some 
constraints relative to each other’s position, then di formation 
is defined as a graph that satisfies these constraints. We then 
introduce a Boolean variable c{si^Sj) to indicate whether two 
agents satisfy these constraints: 

. X f 1 all constraints are satisfied 
= otherwise 

and if c{si,Sj) = 1 we say that agents i and j are connected. 
A loop-free path from i to the leader, ;r; = {0,..., a, ..., 
is defined as an ordered set where neighboring agents are 
connected such that c{sa^sif) = 1. Let 11; be the set of all 
possible paths connected to the leader. The graph ^(s) is 
connected if 11; 7 ^ 0 for all i G J^f- We can now formulate 
an optimal formation problem with connectivity preservation 



Fig. 1: A mission space example where the triangle is the leader and the red line 
is a predefined trajectory in [0,r]. The circles are followers and the rectangle is an 
obstacle. The formation is maintained in [0,ti], but at t 2 a new formation is needed. 


as follows, for any fixed t e [ 0 , T]: 

max H{s{t)) 

s(t)eQ 

s.t. Si(t)eFCQ, ieJf ^2) 

is given 

^{s{t)) is connected 

For the sake of generality, we impose the constraint sft) G 
F C Cl for all follower agents to capture the possibility that 
a formation is constrained. The feasible space F can be 
convex (e.g., followers may be required to be located on one 
side of the leader relative to a line in Cl that goes through 
so{t)) or non-convex (e.g., followers may be forbidden to 
enter polygonal obstacles and F is the set Cl excluding 
all interior points of the obstacles). The solution to this 
problem is an optimal formation at time t and is denoted 
by ^F{^{t)). Given a time interval [hfi], the formation is 
maintained in [t\di] if sft) — Si{t\) = S{){t) — S{){t\) holds 
for all t G [t\f2]^ I ^ otherwise, it is a new formation. 
Figure ?? shows an example of optimal dynamic formation 
control in a mission space with obstacles. Clearly, this is a 
challenging problem. To begin with, the last constraint in ^ 
is imprecise and may be different in a convex or non-convex 
feasible space. In addition, the computational complexity of 
obtaining a solution may be manageable in determining an 
initial formation but becomes infeasible if a new formation 
^/ 7 (s(r)) is required during the real-time execution of a 
mission. In the following two sections, we first propose an 
approach to solve this problem in a convex feasible space 
and then use this solution to enable the maintenance of a 
formation in a non-convex case. 

III. Optimal Dynamic Formation Control in a 
Convex Feasible Space 

In a convex feasible space, the simplest connection con¬ 
straints are of the form dij{t) = || 5 ’;(^) — 5 ’j(r)|| < Qj for 
some pair (iJ), iJ G {0, 1,...,V}, where Qj > 0 is a given 
scalar. This may be the minimum distance needed to establish 
communication or dij may be used to enforce a specific 
desired shape in the formation. Techniques based on the 
graph Laplacian [23] are often used to solve this kind of 
problem, e.g., [24]. However, our goal is to determine a 
formation which solves the optimization problem in Q for 
a given H{s{t)). Thus, we describe next an approach to 



transform the last constraint in 0 into a mixed integer 
nonlinear optimization problem by introducing a set of flow 
variables over ^(s). The leader 0 is assumed to be a 
source node which sends N units of flow through the graph 
^(s) to all other agents. Let ptj G i e JK, j G ^ 
be an integer flow amount through link (ij). Note that, 
in general, pij 7 ^ pjt and that either ptj > 0 or pjt > 0 
implies that c{si,Sj) = 1. We can then deflne a flow vector 
P = (Poi,Pii,Pm, • • ■,p0N, ■ ■ ■,Pnn)- Observe that pio,i G ^ 
is not a flow variable in p since the leader is not allowed to 
receive any flows from the followers. For each follower 7 , 
we deflne an auxiliary variable Nj to be the net flow at node 

j- 

~ ^ Pij ~ ^ Pji 

Using this notation, we introduce next a number of linear 
constraints that represent a connected graph. First, the leader 
provides N units of flow: 

Y. Poi=N (4) 

Next, each follower j must receive a net flow Nj = 1 in order 
to ensure that there is one path from the leader to 7 : 

Nj= E Pij- E = (5) 

To prohibit self loops we require that 

pii = 0 , iG^ (6) 

Finally, the maximal flow capacity is upper bounded by the 
source amount N\ 


Pij<N, iG^, JgjVf (7) 

Observe that Q and Q are linearly dependent since = 
N. Thus, the constraint 0 is redundant and may be omitted. 

Theorem 1 If there exists a flow vector p such that 
constraints 0-0 hold, then there exists a connected graph 
^(s). Moreover, the number of possible graphs is flnite. 

Proof: We use a contradiction argument. Assume that at 
least one follower agent is not connected to the leader while 
satisfying 0 - 0 . We can separate the follower agents into 
two sets: Ni = {k : 0} and A 2 = {7 :117 = 0}. Then, 

pkj = 0 must be true for all k e Ni and 7 G A 2 . This is 
because if pkj > 0 , then there exists a path Tlj = {TlkJ} where 
Tlk eUk, which contradicts the fact that 7 G A 2 . In addition, 
obviously poj = 0 for 7 G A 2 . Summing the left-hand-sides 
of all constraints 0 such that 7 G A 2 , we obtain 


L^j= E f E E p^l 

jeN2 jeN2 ke^p J 


= E Y Pkj +Y Pkj + POj 

jeN2 keN2 



+ Y Pjk 

keN2 


= Y Y Pkj- Y Y pjk “EE Pjk 

j^N2 keN2 j^N2 keN2 j^N2 keNi 


=- Y Ypjk<^ 

jeN2 keNi 


( 8 ) 


Next, summing the right-hand-sides of the constraints 0 
over 7 G N2 we get 'LjeN2 Nj = N > 0- This contradicts the 
constraint 0 leading to the conclusion that the graph W{s) 
is connected. The additional constraints 0-0 are necessary 
to ensure that the number of feasible flow vectors p is flnite. 
Clearly, 0 prohibits self-loops while 0 prevents an inflnite 
number of solutions where edges {ij) in ^(s) may take any 
unbounded flow value pij > 0 . ■ 

Observe that ptj > 0 indicates a connection between agents 
i and 7 . This can be combined with the constraint dij{t) < 
Cij to write Pij{dij{t) — Qj) < 0 for all edges (iJ) in 
^(s). Moreover, the convex set F can be expressed through 
linear constraints. Thus, the optimal formation problem with 
connectivity preservation at any flxed t G [0,r] becomes a 
Mixed Integer Nonlinear Problem (MINLP): 

min -H{s{t),p) 
s(f),p 

s.t. Si{t)GFCQ., i = 0,...,N 

Y Pij - E pji 

ieJF ie^F 

Pij{dij(t^ ^ 5 j ^ 

Pii = 0, / G 
pij<N, ieJK, 

Note that any agent position vector s{t) specifles a graph at 
time t. The role of p is in ensuring that this graph is con¬ 
nected by satisfying the constraints in thus creating an 
optimal formation. However, there is no advance information 
regarding what the optimal formation looks like and how the 
optimal formation changes over time as the leader moves in 
a time interval [ 0 , T] unless is given some speciflc 

structure. 

For the remainder of this paper, we will consider the class 
of coverage control problems [ 6 ], [13]-[19] which impose 
a particular structure on Agents are assumed to 

be equipped with some sensing and some communication 
capabilities. In particular, we assume that agent Vs sensing 
is limited to a set 0^i{t) C Q.. For simplicity, we let 0^i{t) be 
a circle centered at Si{t) with radius 5/. Thus, Q^i{t) = {x : 
di{x,t) < 5;} where di{x,t) = ||v — ||, the standard Eu¬ 

clidean norm. To further maintain simplicity without affect¬ 
ing the generality of the analysis, we set 5i = 5 for all agents. 
We deflne pi{x,Si{t)) to be the probability that i detects an 
event occurring at point v. This function is deflned to have the 
following properties: (/) pi{x,Si{t)) = 0 if v ^ ^i{t), and (//) 
Pi{x,Si{t)) > 0 is a monotonically nonincreasing function of 
di{x^t). The overall sensing detection probability is denoted 
by pi{x,Si{t)) and deflned as 


Pi{x,Si{l)) 


Pi{x,Si{t)) if xGQi{l) 
0 if X ^ Q.i{t) 


( 10 ) 


Note that pi{x,Si{t)) may not be continuous in Si{t). The 
joint detection probability, denoted by P(v,s(r)), captures the 
sensing ability of the entire agent team. That is, an event at 
xeQ^is detected by at least one of the N cooperating agents 




with probability P{x,s{t)) is given by 

N 

P{x,s{t)) = 1 -Hll -Pi(.X,Si{t))] (11) 

i=0 

where we assume that agents sense independently of each 
other. In addition to sensing, the communication capabilities 
of agents are defined by their relative distance: agents i and 
j can establish a communication link if || 5 '/(r) — 5 'y(r)|| < C. 
Thus, in this class of problems a formation is required to 
maintain full communication among agents. Finally, one of 
the agents, indexed by 0 , is designated as the leader whose 
position so{t) is given. 

The objective function for optimal coverage is the same as 
in [18] except for the presence of a leader whose position is 
predefined. For any x G ^1, the function R{x) captures 

an a priori estimate of the frequency of event occurrences at 
V and is referred to as an “event density” satisfying R{x) >0 
for all V G and < oo. In this problem, we assume 

that the event density is a constant for any x G ^1. We are 
interested in maximizing the total detection probability over 
the mission space Q: 

max = [ R{x)P{x,s{t))dx (12) 

s(0 JQ 

SO that the objective in ([^ is = 

JaR{x)P{x,s{t))dx. Figures and show optimal formation 
examples obtained by solving (|^ at time t with so{t) located 
at the center of the mission space. 



Fig. 2: Optimal formation for 5 fol- Fig. 3: Optimal formation for 11 fol¬ 
lowers (numbers) and one leader (L) lowers and a leader. Followers are 
in a bounded mission space. constrained to the left side of the 

leader. 


coordinate system. Given any point x = (xx^Xy) e F, let 
I = {ri^Qi) be the polar coordinates in /’s local coordinate 
system. Then, the transformation that maps (r/,0;) onto the 
global coordinate system is x = Si{t) F [ncos6i rtsinGt]^. 
Upon switching to this local coordinate system, the sensing 
probability becomes pi{x,Si{t)) = Pi{ri) if rt < 5. Since 
G F for all r G [0,r], the local sensing range of Si{t), 
which is denoted by = {{ri,6i) : n < C ,0 < Of < In}, 
is time-invariant. Therefore, recalling ([n), the objective 
function in ( p^ is 


H 


(s{t)) = [ R(x)P(x,s(t))dx 

Ja 

= /.A, _ R{x)P{x,s{t))dx 


Uf=oa(f) 


In , “lid 

i=0 

r ^ 

/ N “lid - Pi{ri)]}dride, 

i=0 


SO that the objective function value remains fixed for any 
t e [0, r]. Since for any agents i and 7 , by assumption, Si{t) — 
Sj{t)= Si (0) + ^0 (0 - *^0 (0) - {sj (0) -1- ^0 (0 - (0)) = Si (0) - 
Sj{0), and s(0) is an optimal solution of (|^, it follows that 
^(s( 0 )) is connected, therefore, is also connected 

and we conclude that s{t) maximizes H{s{t)). ■ 

The implication of Theorem 2 is that when a mission space 
has no obstacles in it or the leader follows a trajectory where 
no obstacles are encountered by any agent, our problem 
is reduced to one of ensuring that all agents accurately 
track the leader’s trajectory. We may discretize time so 
that agents update their locations at 0 < < • < tx = T. 

Assuming that problem is solved at ^ = 0 , an optimal 
formation is obtained and we subsequently strive to maintain 
this formation until a significant “event” occurs such as an 
agent failure, a change in objective function or 

encountering obstacles; at such a point, some amount of 
reconfiguration is required while still aiming to maximize 


A solution of this MINLP is computationally costly so 
that it is not realistic to expect re-solving it over the course 
of a mission t e [O^T] as the leader moves. However, it is 
not always necessary to repeatedly solve this problem over 
[0, r]. Theorem 2 presents a condition under which we only 
need to solve the problem at r = 0. This simply formalizes the 
rather obvious fact that if no new constraints (e.g., obstacles) 
are encountered over r G (0, T], then the optimal formation at 
t = 0 can be preserved by maintaining fixed relative positions 
for all agents. 

Theorem 2 Let s(0) be an optimal solution of problem 
§ 3i t = 0 and assume that Q^i{t) C Fp e ^ and that so{t) 
is known to all followers for all t e (0, T]. If Si{t) = ^'/(O) + 
5'o(0 —5'o(0), i G then s{t) maximizes in (12). 

Proof: Let us introduce a local polar coordinate system for 
each agent /, so that the origin of fs local coordinate system 
is Si and the axes are parallel to those in the mission Cartesian 


IV. Optimal Dynamic Formation Control in a 
Mission Space with Obstacles 

We have thus far solved an optimal dynamic formation 
problem with connectivity constraints in a convex feasible 
space F by solving a MINLP. However, this method may 
fail when F is non-convex, e.g., when F cannot be described 
through linear or nonlinear constraints. In this section, we 
address the optimal dynamic formation problem in a mission 
space with obstacles, thus considering a non-convex feasible 
space. 

We model the obstacles as m non-self-intersecting poly¬ 
gons denoted by Mj, j = l,...,m. The interior of Mj is 
denoted by Mj, so that the overall feasible space is F = 
Q \ (Ml U ... UMm), i.e., the space Q excluding all interior 
points of the obstacles. In this setting, we seek to ensure 
the following two requirements. First, the distance between 
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Fig. 4: An example of a connected network at t and constructed connected network 
by Algorithm 1 at t + e. 



Fig. 5: Two states of the agents network and the transition time points between the 
two states. 


two connected agents must be < C. We define c\{si^Sj) to 
indicate whether this requirement is satisfied: 


Cl{Si,Sj) 


1 |kr-^y||<C 

0 otherwise 


(14) 


Second, the connected agents are required to have a line 
of sight with respect to each other. We define C 2 {si,Sj) to 
indicate this requirement: 

( \ _ / 1 ~ ^ ^ ^ ^ [^5 

C 2 \Si^Sj) < Q otherwise 

(15) 

Agents i and j satisfying ci{si,Sj) = 1 as well as C 2 {si,Sj) = 
1 are referred to as connected. We also define c{si,Sj) = 

Cl{Si,Sj)c2{Si,Sj). 

A version of this connectivity preservation problem was 
addressed in [18], where agents are required to remain con¬ 
nected with a fixed base while at the same time maximizing 
the objective function in ^V2) . A gradient-based algorithm, 
termed Connectivity Preservation Algorithm (CPA), was de¬ 
veloped for agent position updating and it was shown that, 
given an initially connected network and if only one agent 
updates its position at any given time, the CPA preserves 
connectivity. The algorithm is applied iteratively over one 
agent at a time and it converges to a (generally local) 
optimum. The CPA exploits the existence of distributed op¬ 
timization algorithms for optimal coverage to attain optimal 
agent locations while also preserving connectivity to a base 
(details on the CPA and its complexity are provided in [18]). 

Our approach here is to take advantage of the CPA. In 
our problem, however, the conditions for applying the CPA 
do not generally hold; this is because the leader’s motion 
does not take connectivity with its neighbors into account 
and the presence of an obstacle, for example, may cause it 
to disconnect from one or more followers. This is illustrated 
in Fig. 1^ At time t, the agent network shown (represented 
by three blue circles and a blue triangle as the leader) is 
connected. At r + e, the leader (triangle) moves to 5'o(^ + e) 
and if agent 2 moves to the point shown in yellow (as 
expected by Theorem 2), then it becomes disconnected from 
the leader because of the obstacle present. We propose an 
algorithm next to construct a connected graph, which may 


no longer be optimal in the sense of problem (|^ but it 
does provide a valid initial condition for invoking the CPA 
described above (this is illustrated in Fig. as the solid red 
graph). This immediately allows us to iteratively apply the 
CPA so as to obtain a new (locally optimal) formation. 

Clearly, it is also possible to invoke as soon as a 
formation reconfiguration is needed. However, the set F 
is no longer convex and the computational complexity of 
this problem makes it infeasible for the on-line adaptation 
required, whereas the approach we propose and the use of 
the CPA render this process computationally manageable. 
In particular, whereas the MINLP is generally NP hard, in 
the CPA each agent i determines its new position through 
a gradient-based scheme using only its neighbor set and its 
downstream and upstream agent sets relative to the leader 
(formally defined in the next section). When the number of 
agents increases, note that the the number of neighbors of i 
may not be affected. The overall increase in complexity is 
linear in the network size. 

Before proceeding, we identify the precise instants when 
formation reconfiguration is necessary due to obstacles en¬ 
countered by agents as the mission unfolds over [0,r]. 
We define two states that the agent team can be in: (/) 
The constrained state occurs when the sensing capability 
of an agent is hindered by an obstacle, captured by the 
condition (U^o^O H (U“ iM’) / 0, i.e., the intersection of 
the sensed part of Q. and the set of interior points of any 
obstacle is not empty, and (//) The free state corresponding 
to (U^o^O n (U^iM') = Thus, the interval [0,r] is 
partitioned into free and constrained intervals with transitions 
at times t^j^ <tl <tj < ... < < tj^ < ...tj < T. This is de¬ 

scribed in Fig. Next, we consider how to generate optimal 
formations over different alternating intervals [t^^t^'^^) and 

A. Optimal formation control in free states 

When the agent network enters a free state at time 
k = 0,...,z, since (U£p^K0) H=0 for all t e 
and F = Q.\{Mi U... UM^), so Q^i{t) G F for any i 
over t e the optimal formation is maintained based 

on Theorem 2. 



B. Optimal formation control in constrained states 


We begin this subsection with some additional notation 
and definitions. Given a connected graph ^(s), we have 
defined a loop-free path connecting agent i to the leader 
as ;r/ = { 0 ,..., a, ..., an ordered set where neighboring 
agents are connected; we have also defined 11 / to be the set 
of all possible paths connecting i to the leader. Let Tii^k be the 
kth path in 11 / and we use to denote the jth element in 
Let = Uj^k^i{7rj^k) be the set of agents downstream 
from i (further away from the leader 0 ) where 




if i ^ ^i,k^ i ^ J and / = 

0 Otherwise 


We also define the set of upstream agents from i as = 
{j : i G G 0 ,... ,A^}. 

The length of a path Ki^k is defined as 'L(;r/ y^) = 
' \\s^i^ -Sjji+i\\, where \7ti^k\ is the cardinality of Tti^k- 
For agent /, the shortest path connected to the leader is 


7t* = arg min '¥{7ti^k) 

For example in Fig.|^ in the path = {0,2,3}, we have 3 
G ^ 2 , 0 G ^ 2 , = Iko -‘^ 2 || + 1^2 -*^3II; for the path 

^ 3,2 = {0,1,2,3}, we have 'F( 7 r 3 , 2 ) = 11*^0 || + lki -*^211 + 

|| 5'2 — ^ 3 1|. Therefore, 7r{ = ;r 3 ^ is the shortest path from agent 
3 to the leader. 

Let TTi and ttj be two paths. Then, we define Tii -f ttj = 
{Tii^TTk}, where Kk = 7tj\7ti, as an ordered set. Note that 
Tti -f Ttj is generally different from Ttj + Tti because of the 
order involved. Given a connected graph ^(s). We define 

e(^(s)) = ;ri* + ... + 7r* (17) 


to be an ordered set containing a permutation of the agent 
set { 0 , 1 ,...,A^} constructed so as to start with the shortest 
path from 0 to agent 1 , followed by \ and so 
on. It immediately follows from this construction that the 
first element of 2 (^(s)) is 0 and that | 2 (^(s))| =N^\. 
Therefore, we can rewrite 2(^(s)) as 

2 (^(s)) = { 0 ,^ 2 ,---,^A^+i} 

where qj G = 2,... ,A^ + 1. For example, in Fig. 
at time L Q{^{s{t))) = {0,1,2,3}. We show next that 
2 (^(s)) has the following property regarding the order of 
its elements. 

Lemma 1 If qi is the ith element of 2(^(s)) constructed 
from a connected graph ^(s), then there exists qj G ^q. such 
that qj is the jth element of 2(^(s)), and j < i for all qi G 

^F' 

Proof: If for all qj G j > /, we cannot find a subset 
of 2(^(s)) that includes {qj^qt}, qj G ^q^, then there is no 
path connected to qt. This contradicts the assumption that 
2 (^(s)) is constructed from a connected graph. ■ 

We also define a projection of v G on a set A G as 


Pa{x) = argmin||x-y|| 

jeA 

Next, let ?^{si) = {y : y G c{si,y) = 1). Recalling the 
definition of c(•,•), ?^{si) is the set of points with which st 


can establish a connection. For any subset of agents ^ C 
let Z(y) = Ui’er ^i^i) be the union of all connection regions 
for agents in y. For example, in Fig.|^ the grey area is Z(y) 
for y = { 0 , 1 } at time tye. 

We are now ready to deal with the situation where the 
formation is in a constrained state and may lose connectivity 
at time t-\-£ given that the graph ^{s{t)) is connected. In 
particular, suppose that when the leader is about to move to 
5'o(^ + e) and informs the followers, at least one of the agents 
will lose connectivity with the formation. Our task is to 
obtain an optimal formation at t-\-£ and this is accomplished 
in two steps: (/) Construct a connected graph ^(s(r + e)) for 
time tye, and (ii) Use this connected graph ^{s{t-\-£)) as 
an input to invoke the CPA. Step (/) is crucial because of the 
fact that the CPA relies on an initially connected graph before 
it can be executed to seek (locally) optimal agent locations 
which still preserve connectivity. This first step is carried out 
by constructing a connected graph through Algorithm We 


Algorithm 1 Connected Graph Construction Algorithm 
Input: Graph ^(s(t)), so(t-i-£) 

Output: Graph ^(s(t-{-£)) 

Initialization: '^•,^/ for i G .aF, Y = {0}, Q{^{s{t))) = 
{Q,q2,...,qN+i} using 
For agent i = qjJ = 2,... ,N 1 
Do the following procedure: 

1: Generate a candidate next location for /: st = 5'/(r) + A^. 

2: For all agents v G if c{si,Sv{t y £)) = 0, go to 

Step else, go to Step 

3: Project Si onto Z(^-n^). Set Si = P^^.f^y){si). 

4 : Set Si{t e) = Si. 

5: Add i to y 

End 


use AL{t) = so{t y e) — so{t) to denote the position change 
vector of the leader from t to t -\-£, where we assume that 
followers have the Aif) information available at t. 

Theorem 3 ^(s(r + e)) obtained by Algorithm is 

connected. 

Proof: Since W{s{t)) is connected, / 0 for i G We 
then use induction to prove that the graph constructed by 
agents in y remains connected at Step in every iteration. 
Initially, y = {Of which is connected. Next, assuming there 
are n agents in y and the graph they form is connected, we 
will prove that after adding the (^+ l)th agent, say i, the 
graph remains connected. 

The addition of i to y occurs at Step 5. There are two 
possible sequences for reaching this step: p][ 2 p| and m 
[3]|^ At Step ^i{^y 7 ^ 0 because of the property of 
2(^(s)) in Lemma 1. It follows that before i performs the 
procedure, there is at least one upstream agent in y. In 
the p][ 2 p | sequence, there exists some m ey D^i such that 
c{si,Sm{t y £)) = 1. Therefore, all agents in y including i 
will be connected. In the [T][2][3]|^ sequence, at Step agent 
/’s position is projected onto the connection ranges of all 
V e y n^i. It follows that the graph formed by agents in 






{y^i} is connected. Step 5 adds agents to Y one by one 
until y = therefore, the graph ^(s(r+ e)) is connected. 


Obviously, Algorithm does not provide a unique way 
to construct a connected graph. For example, the formation 
could be adjusted to a line or a star configuration with S{){t + 
e) as the center of the star. However, this would entail a 
major formation restructuring whereas in Algorithm we 
seek to retain the closest possible formation to the original 
(optimal) one by setting candidate locations as seen in Step 
If such a candidate is not feasible, then the agent will move 
a minimal distance (in the projection sense) to be connected. 

Once step (/) above is completed by obtaining this con¬ 
nected graph ^(s(r)), step (//) is performed by invoking 
the CPA to optimize the agent locations within the new 
formation. Clearly, once obstacles are cleared and the agent 
team re-enters a free state (see Fig. [^, we may revert to the 
original optimal formation. 


V. Simulation Results 

In this section, we provide a simulation example illustrat¬ 
ing what the optimal formation maximizing coverage in a 
mission space with obstacles looks like and how it changes 
at some significant instants. 

We choose the event density functions to be uniform, i.e., 
R{x) = 1. The mission space is a 60 x 50 rectangle. The 
distance constraint is C = 10 and the sensing range of each 
agent is 5 = 8. At every step, the leader moves to the right 
one distance unit per unit of time. The mission space is 
colored from dark to lighter as the joint detection probability 
decreases (the joint detection probability is > 0.50 for green 
areas, and near zero for white areas). The leader (labeled 
“L”) moves along a predefined trajectory (the purple dashed 
line). There are 8 followers, indicated by numbers, which are 
restricted to locations on the left side of the leader during 
any movement. 

Figures m\ show snapshots of the process at selected 
events of interest over [0, T]. Figure shows the initial 
configuration at t = 0, where the agent team is located in a 
convex feasible space. As shown in Sec. Ill, in this case, the 
optimal formation can be obtained by solving a MINLP [25]. 
In the results shown, we have used TOMLAB, a MATLAB- 
based optimization solver. For the non-convex objective 
function defined in the solution is usually a local 

maximum; we sought to find the best local (possibly global) 
optimum possible by implementing a multi-start algorithm 
on the solver. This is done at the start of the mission, when 
an off-line computationally intensive procedure is possible. 
Moreover, this local maximum can be improved by applying 
the CPA; in fact, in this example the use of the CPA led 
to an improvement from H{s) = 741.5 to H{s) = 816.7, as 
shown in Fig. [7] Thus, in general, supplying the CPA with 
an initial connected graph obtained by solving the MINLP 
enables it to converge to a better value. For example. Fig. 
^ is a local maximum attained by starting with a star-like 


connected graph shown in Fig. 12 with the objective function 
value Ff(s) = 781.1 (although this is still worse than the value 
in Fig. 1^. 



Fig. 6: At r = 0, optimal formation from Fig. 7: At t = 0, optimal formation im- 
MINLP, H{s) = 741.5 proved by CPA of Fig. 1^ with H{s) = 

816.7 



Fig. 8: At t = 5, agent 5 needs projection Fig. 9: Ait = 1, apply CPA after proj co¬ 
in Step 3 of Algorithm 1 tion in[^ the structure of the tree doesn’t 

change 



Fig. 10: Ai t = 12, the structure of the Fig. 11: At t = 35, the end of the mission 
tree changes 



Fig. 12: A star-like connected graph Fig. 13: Apply CPA from H{s) = 

781.1 


In the time interval [0,5], the formation is maintained. 
At r = 5, agent 5 is located at a vertex of an obstacle and 
will therefore lose connectivity as the leader moves to the 
next step at t = 6. At this point, agent 5 will determine 
its next position ss{6) by applying a projection at Step 
of Algorithm Note that only agent 5 needs to perform 
this projection, rather than the whole team of agents, hence 
the computational effort is minimal. Figure captures the 
optimal formation following Fig. 

Observe that over the period [0,12), although the optimal 
formation remains a tree, it is no longer the same as the 
original one. However, for each agent /, its downstream 
node set and upstream node set remain unchanged. 
At r = 12, clearly, the structure of the formation has been 
changed. This is a consequence of either the projection step 
in Algorithm 1 or the CPA. At the end of the mission at 
t = 35, the formation is shown in Fig. 11 The agents seek 












to form a line to go through the narrow region of the mission 
space while at the same time maximizing coverage. During 
the remaining interval [12,35], the process is similar to what 
is seen over [5,12]. 

As we pointed out in the last section, constructing a 
connected graph can be accomplished in a variety of ways. 
As shown in Fig.[^ a star-like graph is an inferior formation 
to that of Fig. [Tj this is expected since the latter was obtained 
specifically to maximize the objective function in ( p^ . In 
addition, a reconfiguration process as shown in Fig. 
requires agents to move longer distances, hence consuming 
more energy. 

VI. Conclusions and future work 

We have addressed the issue of optimal dynamic formation 
of multi-agent systems in mission spaces with obstacles. 
When the agent team is in a free state (no obstacles in the 
mission space affecting them), a locally optimal solution of a 
MINLP can provide an initial formation that agents maintain 
or it is a good initial point for using the CPA (developed in 
prior work [18]) to obtain a better local optimum. When the 
feasible space is non-convex and connectivity is lost, we have 
developed an algorithm to construct a connected graph as an 
input for the CPA while seeking to maintain the original 
formation with minimal effort. 

Future work aims at investigating optimal dynamic forma¬ 
tion control for more general classes of objective functions, 
beyond the coverage control problem. 
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